# author = "Ronald"
# date   = 24-04-2010 07:29:02 PM

from OpenGL.GL import *
from OpenGL.GLUT import *
from OpenGL.GLU import *
from OpenGL.GL import glScale
from numpy import *
# import sys
# import math

windowId = -1
WIDTH = 800
HEIGHT = 600

# 'angulos del brazo y el antebrazo, en y en en z
ARMY = 0.0
ARMZ = 0.0
FOREARMY = 0.0
FOREARMZ = 0.0

# proporciones del largo de las partes del brazo
ARM = 2.0
FOREARM = 2.0
ELBOW = 1.0
SHOULDER = 1.0

# proporci'on para hacer el brazo a escala
SIZE = 0.5

#funciones de movimiento del brazo
# mueve el codo hacia arriba
def FOREARMYAdd():
    global FOREARMY
    if FOREARMY < 90:
        FOREARMY = (FOREARMY + 5) % 360

# mueve el codo hacia abajo
def FOREARMYSubstract():
    global FOREARMY
    if FOREARMY > 0:
        FOREARMY = (FOREARMY - 5) % 360

# mueve el hombro hacia arriba
def ARMYAdd():
    global ARMY
    if ARMY < 90 or ARMY >= 270:
        ARMY = (ARMY + 5) % 360

# mueve el hombro hacia abajo
def ARMYSubstract():
    global ARMY
    if ARMY <= 90 or ARMY > 270:
        ARMY = (ARMY - 5) % 360

# mueve el hombro hacia delante
def ARMZAdd():
    global ARMZ
    if ARMZ >= 270:
        ARMZ = (ARMZ + 5) % 360

def ARMZSubstract():
    global ARMZ
    if ARMZ > 270 or ARMZ == 0:
        ARMZ = (ARMZ - 5) % 360

# Configuramos OpenGL
def InitGL():

    glClearColor(0.0, 0.0, 0.0, 0.0)	# Color negro, sin transparencia

    glClear(GL_COLOR_BUFFER_BIT)
    glViewport(0, 0, WIDTH, HEIGHT)
    glMatrixMode(GL_PROJECTION)
    glLoadIdentity()
    gluOrtho2D(-1, 1, -1, 1)

    glMatrixMode(GL_MODELVIEW)
    glLoadIdentity()

def display():
    glClear(GL_COLOR_BUFFER_BIT)
    glColor3f (0.0, 0.0, 1.0)
    #glLoadIdentity()                # clear the matrix

    # dibujar el hombro
    glPushMatrix()
    glScalef(SIZE, SIZE, SIZE)
    #glTranslatef (-ARM/2, 0.0, 0.0)
    glTranslatef(SHOULDER/2, 0.0, 0.0)
    glRotatef(ARMZ, 0.0, 1.0, 0.0)
    glRotatef(ARMY, 0.0, 0.0, 1.0)
    glutWireSphere(SHOULDER/2, 10, 10)      # ARMY
    glPopMatrix()

    # dibujar el brazo
    glColor3f (1.0, 0.0, 0.0)
    glPushMatrix()
    glScalef(SIZE, SIZE, SIZE)
    #glTranslatef (-ARM/2, 0.0, 0.0)
    glTranslatef (SHOULDER/2, 0.0, 0.0)
    glRotatef(ARMZ, 0.0, 1.0, 0.0)
    glRotatef(ARMY, 0.0, 0.0, 1.0)
    glTranslatef (ARM/2 + SHOULDER/2, 0.0, 0.0)
    glScalef(ARM, 1, 1)
    glutWireCube(1)      # ARMY
    glPopMatrix()

    # dibujar el codo
    glColor3f (0.0, 0.0, 1.0)
    glPushMatrix()
    glScalef(SIZE, SIZE, SIZE)
    #glTranslatef (-ARM/2, 0.0, 0.0)
    glTranslatef (SHOULDER/2, 0.0, 0.0)
    glRotatef(ARMZ, 0.0, 1.0, 0.0)
    glRotatef(ARMY, 0.0, 0.0, 1.0)
    glTranslatef(ELBOW/2 + ARM + SHOULDER/2, 0.0, 0.0)
    glutWireSphere(ELBOW/2, 10, 10)      # ARMY
    glPopMatrix()

    # dibujar el antebrazo
    glColor3f (1.0, 0.0, 0.0)
    glPushMatrix()
    glScalef(SIZE, SIZE, SIZE)
    #glTranslatef (-1.0, 0.0, 0.0)
    glTranslatef (SHOULDER/2, 0.0, 0.0)
    glRotatef(ARMZ, 0.0, 1.0, 0.0)
    glRotatef(ARMY, 0.0, 0.0, 1.0)
    glTranslatef (ELBOW/2 + ARM + SHOULDER/2, 0.0, 0.0)
    glRotatef (FOREARMY, 0.0, 0.0, 1.0)
    glTranslatef (FOREARM/2 + ELBOW/2, 0.0, 0.0)
    glScalef(FOREARM, 1, 1)
    glutWireCube(1)      # FOREARMY
    glPopMatrix()

    glFlush();

    glutSwapBuffers()

def myinit():
    glShadeModel (GL_FLAT)

def myReshape(w, h):
    glViewport(0, 0, w, h)
    glMatrixMode(GL_PROJECTION)
    glLoadIdentity()
    gluPerspective(65.0, w/h, 1.0, 20.0)
    glMatrixMode(GL_MODELVIEW)
    glLoadIdentity()
    glTranslatef(0.0, 0.0, -5.0)

def keyPressed(*args):

    global ARMY
    global FOREARMY

    key = args[0]

    # Teclas para mover el brazo
    if key == '\033':   # tecla escape
	    glutDestroyWindow(windowId)
	    sys.exit()
    if key == 'w':
        print ARMY
	ARMYAdd()
    if key == 's':
        print ARMY
	ARMYSubstract()
    if key == 'd':
        print FOREARMY
	FOREARMYSubstract()
    if key == 'a':
	print FOREARMY
        FOREARMYAdd()
    if key == 'e':
        print ARMZ
	ARMZAdd()
    if key == 'q':
        print ARMZ
	ARMZSubstract()

def main():
    global windowId

    # Configurar OpenGL
    glutInit(())

    # Mi modo de Display. Por ahora, no importa
    glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_ALPHA )

    # Tamanio de la ventana.
    glutInitWindowSize(WIDTH, HEIGHT)

    # Donde queremos que se posicione la ventana al iniciar.
    glutInitWindowPosition(0, 0)
    windowId = glutCreateWindow("Proyecto Robot - Marcel_Ronald")

    # Defino la funci'on que se encargar'a de dibujar
    glutDisplayFunc(display)

    # La funci'on cuando el programa este Idle.
    # En un videoJuego, 'este ser'ia el lugar donde actualizo posiciones, calculo puntajes, etc.
    glutIdleFunc(display)

    # La funci'on a llamar cuando cambia el tamanio de la ventana.
    glutReshapeFunc (myReshape)

    # La funci'on que se encarga de recibir el input desde teclado
    glutKeyboardFunc (keyPressed)

    # Configuramos OpenGL para trabajar al tamanio deseado.
    myinit()

    # Loop infinito
    glutMainLoop()

main()